#include "polhemusControl.h"
#include <iostream>

using namespace std;
/*********************************************************************************
 *WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS*
 *WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS* 
 *WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS* 
 *WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS* 
 *WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS WINDOWS* 
 *****************************************************************************/
#if defined (_WIN32)


polhemusControl::polhemusControl()
{
}

polhemusControl::polhemusControl(const polhemusControl &pC){
	g_pdiMDat = pC.g_pdiMDat;
	g_bCnxReady = pC.g_bCnxReady;
	g_dwFrameSize = pC.g_dwFrameSize;
	g_dwStationMap = pC.g_dwStationMap;
	g_pdiSer = pC.g_pdiSer;
	g_pdiDev = pC.g_pdiDev;

	x1 = pC.x1;
	y1 = pC.y1;
	z1 = pC.z1;

	rx1 = pC.rx1;
	ry1 = pC.ry1;
	rz1 = pC.rz1;

	x2 = pC.x2;
	y2 = pC.y2;
	z2 = pC.z2;

	rx2 = pC.rx2;
	ry2 = pC.ry2;
	rz2 = pC.rz2;
}

void polhemusControl::InicializarPolhemus()
{
    g_pdiMDat.Empty();
    g_pdiMDat.Append( PDI_MODATA_POS );
    g_pdiMDat.Append( PDI_MODATA_ORI );
    g_pdiMDat.Append( PDI_MODATA_POS );
    g_pdiMDat.Append( PDI_MODATA_ORI );
    g_pdiMDat.Append( PDI_MODATA_FRAMECOUNT );
    g_dwFrameSize = 8+12+12+4;
    //g_dwFrameSize = 72;
    g_bCnxReady = FALSE;
    g_dwStationMap = 0;


    
    g_pdiDev = new CPDIdev;	
    
    //Conectandose al Polhemus
    if (!(g_pdiDev->CnxReady()))
    {
        g_pdiDev->SetSerialIF( &g_pdiSer );
        ePiCommType eType = g_pdiDev->DiscoverCnx();
        
        g_bCnxReady = g_pdiDev->CnxReady();
    }
    else
    {
        g_bCnxReady = TRUE;
    }
    
    if(g_bCnxReady == FALSE)
    {
        // No se logro conexion, terminar o avisar
        printf("Error en la conexion");
        
    }
    else
    {
        // Conexion exitosa, avisar
        printf("Conexion con polhemus \n");
    }
    
    // Configuramos el Polhemus
    
    g_pdiDev->SetSHemiTrack(-1);
    g_pdiDev->SetSDataList( -1, g_pdiMDat );
    
    CPDIbiterr cBE;
    g_pdiDev->GetBITErrs( cBE );
    
    CHAR sz[100];
    cBE.Parse( sz, 100 );
    if (!(cBE.IsClear()))
    {
        g_pdiDev->ClearBITErrs();
    }
    
    g_pdiDev->GetStationMap( g_dwStationMap );
    
    
    if (!(g_pdiDev->StartContPno(0)))
    {
    }
    
    this->isOpen = true;

}

void polhemusControl::DesconectarPolhemus()
{
    // Desconectamos el Polhemus
    if (!(g_pdiDev->StopContPno()))
    {
    }
    
    if (g_pdiDev->CnxReady())
    {
        g_pdiDev->Disconnect();
    }

}

void polhemusControl::ActualizarDatos()
{
    // Polhemus data vars
    PBYTE pBuf;
    DWORD dwSize;
    
    // Read Polhemus data
    if(!(g_pdiDev->LastPnoPtr(pBuf, dwSize)))
    {
        printf("error obteniendo datos\n");
    }
    else if((pBuf == 0) || (dwSize == 0))
    {
        printf("error en los apuntadores\n");
    }
    else
    {
        int intContadorSensor = 1;
        unsigned int i = 0;
        while ( i< dwSize)
        {
            BYTE ucSensor = pBuf[i+2];
            SHORT shSize = pBuf[i+6];
            
            // skip rest of header
            i += 8;
            
            PFLOAT pPno = (PFLOAT)(&pBuf[i]);
            
            //printf("%2d   %+011.6f %+011.6f %+011.6f   %+011.6f %+011.6f %+011.6f\n", 
            //				ucSensor, pPno[0], pPno[1], pPno[2], pPno[3], pPno[4], pPno[5] );
            
            
            i += shSize;
            
            if(intContadorSensor == 1)
            {
                x1 = pPno[0];
                y1 = pPno[1];
                z1 = pPno[2];
                rx1 = pPno[3];
                ry1 = pPno[4];
                rz1 = pPno[5];
            }
            else if(intContadorSensor == 2)
            {
                x2 = pPno[0];
                y2 = pPno[1];
                z2 = pPno[2];
                rx2 = pPno[3];
                ry2 = pPno[4];
                rz2 = pPno[5];
            }
            
            //printf("x: %f y: %f z %f \n",x,y,z);
            
            intContadorSensor++;
        }
	
    }

}
#endif
/*******************************************************************************
 *LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX*
 *LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX* 
 *LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX* 
 *LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX* 
 *LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX LINUX* 
 *******************************************************************************/
#if defined (__linux__)

polhemusControl *polhemusControl::p = NULL;

/* void VRPN_CALLBACK polhemusControl::handle_tracker_pos_quat (void *userdata, const vrpn_TRACKERCB t){
      printf("Tracker %s, sensor %id:\n        pos (%5.2f, %5.2f, %5.2f); quat (%5.2f, %5.2f, %5.2f, %5.2f)\n",
              "pruebapolhemus",
               t.sensor,
               t.pos[0], t.pos[1], t.pos[2],
               t.quat[0], t.quat[1], t.quat[2], t.quat[3]);
        p->set_x1(t.pos[0]);
        p->set_y1(t.pos[1]);
        p->set_z1(t.pos[2]);
        p->set_rx1(t.pos[0]);
        p->set_ry1(t.pos[1]);
        p->set_rz1(t.pos[2]);
}
*/

void polhemusControl::ActualizarDatos(){
    /*  
    char *name = "Tracker0@DarkStar";
    vrpn_Tracker_Remote *tkr = new vrpn_Tracker_Remote(name);
    
    if(tkr == NULL){
        cout << "ERROR abriendo el dispositivo" << endl;
    }
    tkr->register_change_handler(NULL, handle_tracker_pos_quat);
    
    int cont = 0;
    while (cont < 4){
        tkr->mainloop();
        vrpn_SleepMsecs(1); // para que no se coma la maquina
        cont ++;
        }*/
}



void polhemusControl::DesconectarPolhemus(){
    
}

void polhemusControl::InicializarPolhemus(){

    p = this;
}

polhemusControl::polhemusControl()
{

}











#endif



